#include <functional>
#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "test_interfaces/srv/test.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

class TestService : public rclcpp::Node
{
private:
    int sum = 0;
    rclcpp::Service<test_interfaces::srv::Test>::SharedPtr service_;
    rclcpp::CallbackGroup::SharedPtr callback_group;

public:
    TestService();
    ~TestService();
    void add_callback(const test_interfaces::srv::Test::Request::SharedPtr request,
                      const test_interfaces::srv::Test::Response::SharedPtr response);
};

TestService::TestService() : Node("test_service")
{
    // 实例化回调组, 作用为避免死锁(请自行百度ROS2死锁)
    callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    service_ = this->create_service<test_interfaces::srv::Test>("add_three_ints",
                                                                std::bind(&TestService::add_callback, this, _1, _2),
                                                                rmw_qos_profile_services_default,
                                                                callback_group);
}

TestService::~TestService()
{
}

void TestService::add_callback(const test_interfaces::srv::Test::Request::SharedPtr request,
                               const test_interfaces::srv::Test::Response::SharedPtr response)
{
    RCLCPP_INFO(this->get_logger(), "doing service");
    int a = request->a;
    int b = request->b;
    int c = request->c;

    int sum = a + b + c;
    response->sum = sum;
    RCLCPP_INFO(this->get_logger(), "service finished, result is %d", sum);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto service = std::make_shared<TestService>();

    // 把节点的执行器变成多线程执行器, 避免死锁
    rclcpp::executors::MultiThreadedExecutor exector;
    exector.add_node(service);
    exector.spin();

    rclcpp::shutdown();
    return 0;
}